diff --git a/csc.h b/csc.h
new file mode 100644
index 0000000..49705cc
--- /dev/null
+++ b/csc.h
@@ -0,0 +1,72 @@
+/*
+ * Copyright (c) 2015-2016 Andreas Baierl <ichgeh@imkreisrum.de>
+ *
+ * This library is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU Lesser General Public
+ * License as published by the Free Software Foundation; either
+ * version 2.1 of the License, or (at your option) any later version.
+ *
+ * This library is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+ * Lesser General Public License for more details.
+ *
+ * You should have received a copy of the GNU Lesser General Public
+ * License along with this library; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
+ *
+ */
+
+#ifndef __CSC_H__
+#define __CSC_H__
+
+#include "vdpau_private.h"
+
+typedef float csc_m[3][4];
+
+#ifdef CSC_FULL_RANGE
+/*
+ * This matrizes are from vl_csc.c from the mesa project
+ * The calculation routines are there, too.
+ */
+
+/* Full range: RGB is in 0~255 */
+static const csc_m cs_bt601 = {
+	{ 1.164f,  0.0f,     1.596f,   0.0f, },
+	{ 1.164f,  -0.391f,  -0.813f,  0.0f, },
+	{ 1.164f,  2.018f,   0.0f,     0.0f, }
+};
+static const csc_m cs_bt709 = {
+	{ 1.164f,  0.0f,     1.793f,   0.0f, },
+	{ 1.164f,  -0.213f,  -0.534f,  0.0f, },
+	{ 1.164f,  2.115f,   0.0f,     0.0f, }
+};
+static const csc_m cs_smpte_240m = {
+	{ 1.164f,  0.0f,     1.794f,   0.0f, },
+	{ 1.164f,  -0.258f,  -0.543f,  0.0f, },
+	{ 1.164f,  2.079f,   0.0f,     0.0f, }
+};
+const float ybias = -16.0f / 255.0f;
+#else
+/* Normal range: RGB is in 16~235 */
+static const csc_m cs_bt601 = {
+	{ 1.0f,    0.0f,     1.371f,   0.0f, },
+	{ 1.0f,    -0.336f,  -0.698f,  0.0f, },
+	{ 1.0f,    1.732f,   0.0f,     0.0f, }
+};
+static const csc_m cs_bt709 = {
+	{ 1.0f,    0.0f,     1.540f,   0.0f, },
+	{ 1.0f,    -0.183f,  -0.459f,  0.0f, },
+	{ 1.0f,    1.816f,   0.0f,     0.0f, }
+};
+static const csc_m cs_smpte_240m = {
+	{ 1.0f,    0.0f,     1.582f,   0.0f, },
+	{ 1.0f,    -0.228f,  -0.478f,  0.0f, },
+	{ 1.0f,    1.833f,   0.0f,     0.0f, }
+};
+static const float ybias = 0.0f;
+#endif
+const float cbbias = -128.0f / 255.0f;
+const float crbias = -128.0f / 255.0f;
+
+#endif
diff --git a/sunxi_disp.c b/sunxi_disp.c
index 89a029b..6cd934f 100644
--- a/sunxi_disp.c
+++ b/sunxi_disp.c
@@ -21,6 +21,7 @@
 #include <stdlib.h>
 #include <stdint.h>
 #include <unistd.h>
+#include <math.h>
 #include <sys/ioctl.h>
 #include "kernel-headers/sunxi_disp_ioctl.h"
 #include "vdpau_private.h"
@@ -202,24 +203,38 @@ static int sunxi_disp_set_video_layer(struct sunxi_disp *sunxi_disp, int x, int
 
 	ioctl(disp->fd, DISP_CMD_LAYER_OPEN, args);
 
-	// Note: might be more reliable (but slower and problematic when there
-	// are driver issues and the GET functions return wrong values) to query the
-	// old values instead of relying on our internal csc_change.
-	// Since the driver calculates a matrix out of these values after each
-	// set doing this unconditionally is costly.
 	if (surface->csc_change)
 	{
-		ioctl(disp->fd, DISP_CMD_LAYER_ENHANCE_OFF, args);
-		args[2] = 0xff * surface->brightness + 0x20;
-		ioctl(disp->fd, DISP_CMD_LAYER_SET_BRIGHT, args);
-		args[2] = 0x20 * surface->contrast;
-		ioctl(disp->fd, DISP_CMD_LAYER_SET_CONTRAST, args);
-		args[2] = 0x20 * surface->saturation;
-		ioctl(disp->fd, DISP_CMD_LAYER_SET_SATURATION, args);
-		// hue scale is randomly chosen, no idea how it maps exactly
-		args[2] = (32 / 3.14) * surface->hue + 0x20;
-		ioctl(disp->fd, DISP_CMD_LAYER_SET_HUE, args);
-		ioctl(disp->fd, DISP_CMD_LAYER_ENHANCE_ON, args);
+		uint32_t b, c, s, h;
+
+		/* ioctl(disp->fd, DISP_CMD_LAYER_ENHANCE_OFF, args); */
+
+		/* scale VDPAU: -1.0 ~ 1.0 to SUNXI: 0 ~ 100 */
+		b = args[2] = ((surface->brightness + 1.0) * 50.0) + 0.5;
+		/* ioctl(disp->fd, DISP_CMD_LAYER_SET_BRIGHT, args); */
+
+		/* scale VDPAU: 0.0 ~ 10.0 to SUNXI: 0 ~ 100 */
+		if (surface->contrast <= 1.0)
+			c = args[2] = (surface->contrast * 50.0) + 0.5;
+		else
+			c = args[2] = (50.0 + (surface->contrast - 1.0) * 50.0 / 9.0) + 0.5;
+		/* ioctl(disp->fd, DISP_CMD_LAYER_SET_CONTRAST, args); */
+
+		/* scale VDPAU: 0.0 ~ 10.0 to SUNXI: 0 ~ 100 */
+		if (surface->saturation <= 1.0)
+			s = args[2] = (surface->saturation * 50.0) + 0.5;
+		else
+			s = args[2] = (50.0 + (surface->saturation - 1.0) * 50.0 / 9.0) + 0.5;
+		/* ioctl(disp->fd, DISP_CMD_LAYER_SET_SATURATION, args); */
+
+		/* scale VDPAU: -PI ~ PI   to SUNXI: 0 ~ 100 */
+		h = args[2] = (((surface->hue / M_PI) + 1.0) * 50.0) + 0.5;
+		/* ioctl(disp->fd, DISP_CMD_LAYER_SET_HUE, args); */
+
+		/* ioctl(disp->fd, DISP_CMD_LAYER_ENHANCE_ON, args); */
+		VDPAU_DBG("Presentation queue csc change");
+		VDPAU_DBG("display driver -> bright: %d, contrast: %d, saturation: %d, hue: %d", b, c, s, h);
+		VDPAU_DBG("Not changing display settings to workaround csc brightness bug");
 		surface->csc_change = 0;
 	}
 
diff --git a/vdpau_private.h b/vdpau_private.h
index bb078ac..d3bdda5 100644
--- a/vdpau_private.h
+++ b/vdpau_private.h
@@ -22,6 +22,7 @@
 
 #define DEBUG
 #define MAX_HANDLES 64
+#define CSC_FULL_RANGE 1
 #define VBV_SIZE (1 * 1024 * 1024)
 
 #include <stdlib.h>
@@ -98,6 +99,8 @@ typedef struct
 	float contrast;
 	float saturation;
 	float hue;
+	int custom_csc;
+	VdpCSCMatrix csc_matrix;
 } mixer_ctx_t;
 
 #define RGBA_FLAG_DIRTY (1 << 0)
diff --git a/video_mixer.c b/video_mixer.c
index 9fd1ccc..cd65be3 100644
--- a/video_mixer.c
+++ b/video_mixer.c
@@ -18,9 +18,90 @@
  */
 
 #include <math.h>
+#include <string.h>
 #include <cedrus/cedrus.h>
 #include "vdpau_private.h"
 #include "rgba.h"
+#include "csc.h"
+
+static VdpColorStandard color_standard;
+
+static void set_csc_matrix(mixer_ctx_t *mix, VdpColorStandard standard)
+{
+	float asin;
+	static const csc_m *cstd;
+
+	mix->csc_change = 1;
+	switch (standard) {
+		case VDP_COLOR_STANDARD_ITUR_BT_709:
+			cstd = &cs_bt709;
+			break;
+		case VDP_COLOR_STANDARD_SMPTE_240M:
+			cstd = &cs_smpte_240m;
+			break;
+		case VDP_COLOR_STANDARD_ITUR_BT_601:
+		default:
+			cstd = &cs_bt601;
+			break;
+	}
+	VdpCSCMatrix *matrix = &mix->csc_matrix;
+
+	if ((*matrix)[1][0] == 0 && (*matrix)[1][1] == 0 && (*matrix)[1][2] == 0)
+	{
+		/* At least contrast was 0.0f. Set Hue and saturation to default. They cannot be guessed... */
+		mix->contrast = 0.0f;
+		mix->hue = 0.0f;
+		mix->saturation = 1.0f;
+	}
+	else
+	{
+		/* Contrast */
+		mix->contrast = (*matrix)[0][0] / (*cstd)[0][0];
+
+		if ((*matrix)[1][1] == 0 && (*matrix)[1][2] == 0)
+		{
+			/* Saturation was 0.0f. Set Hue to default. This cannot be guessed... */
+			mix->hue = 0.0f;
+			mix->saturation = 0.0f;
+		}
+		else
+		{
+			/* Hue */
+			asin = asinf(sqrtf(pow(((*matrix)[1][1] * (*cstd)[1][2] - (*matrix)[1][2] * (*cstd)[1][1]), 2.0) /
+			       (pow((-(*matrix)[1][1] * (*cstd)[1][1] - (*matrix)[1][2] * (*cstd)[1][2]), 2.0) +
+			        pow(((*matrix)[1][1] * (*cstd)[1][2] - (*matrix)[1][2] * (*cstd)[1][1]), 2.0))));
+
+			if (((*matrix)[2][1] < 0 && (*cstd)[2][1] < 0) || ((*matrix)[2][1] > 0 && (*cstd)[2][1] > 0))
+				if (((*matrix)[0][1] < 0 && (*matrix)[0][2] > 0) || ((*matrix)[0][1] > 0 && (*matrix)[0][2] < 0))
+					mix->hue = asin;
+				else
+					mix->hue = - asin;
+			else
+				if (((*matrix)[0][1] < 0 && (*matrix)[0][2] > 0) || ((*matrix)[0][1] > 0 && (*matrix)[0][2] < 0))
+					mix->hue = - M_PI + asin;
+				else
+					mix->hue = M_PI - asin;
+
+			/* Check, if Hue was M_PI or -M_PI */
+			if ((fabs(fabs(mix->hue) - M_PI)) < 0.00001f)
+				mix->hue = - mix->hue;
+
+			/* Saturation */
+			mix->saturation = (*matrix)[1][1] / (mix->contrast * ((*cstd)[1][1] * cosf(mix->hue) - (*cstd)[1][2] * sinf(mix->hue)));
+		}
+	}
+
+	/* Brightness */
+	mix->brightness = ((*matrix)[1][3] -
+	                  (*cstd)[1][1] * mix->contrast * mix->saturation * (cbbias * cosf(mix->hue) + crbias * sinf(mix->hue)) -
+	                  (*cstd)[1][2] * mix->contrast * mix->saturation * (crbias * cosf(mix->hue) - cbbias * sinf(mix->hue)) -
+	                  (*cstd)[1][3] - (*cstd)[1][0] * mix->contrast * ybias) / (*cstd)[1][0];
+
+	VDPAU_DBG("Setting mixer value from following color standard: %d", standard);
+	VDPAU_DBG(">mix->bright: %2.3f, mix->contrast: %2.3f, mix->saturation: %2.3f, mix->hue: %2.3f",
+	          (double)mix->brightness, (double)mix->contrast,
+	          (double)mix->saturation, (double)mix->hue);
+}
 
 VdpStatus vdp_video_mixer_create(VdpDevice device,
                                  uint32_t feature_count,
@@ -39,8 +120,16 @@ VdpStatus vdp_video_mixer_create(VdpDevice device,
 		return VDP_STATUS_RESOURCES;
 
 	mix->device = dev;
+	mix->brightness = 0.0;
 	mix->contrast = 1.0;
 	mix->saturation = 1.0;
+	mix->hue = 0.0;
+
+	/* CSC: Use BT601 at initalization time */
+	mix->custom_csc = 0;
+	color_standard = VDP_COLOR_STANDARD_ITUR_BT_601;
+	vdp_generate_csc_matrix(NULL, color_standard, &mix->csc_matrix);
+	set_csc_matrix(mix, color_standard);
 
 	return VDP_STATUS_OK;
 }
@@ -190,26 +279,6 @@ VdpStatus vdp_video_mixer_get_feature_enables(VdpVideoMixer mixer,
 	return VDP_STATUS_ERROR;
 }
 
-static void set_csc_matrix(mixer_ctx_t *mix, const VdpCSCMatrix *matrix)
-{
-	mix->csc_change = 1;
-	// default contrast for full-range has 1.0 as luma coefficients
-	mix->contrast = ((*matrix)[0][0] + (*matrix)[1][0] + (*matrix)[2][0]) / 3;
-	// the way brightness and contrast work with this driver, brightness
-	// is the brightness of a "black" pixel
-	mix->brightness = ((*matrix)[0][1] + (*matrix)[1][1] + (*matrix)[2][1]) / 2 +
-	                  ((*matrix)[0][2] + (*matrix)[1][2] + (*matrix)[2][2]) / 2 +
-	                  (*matrix)[0][3] + (*matrix)[1][3] + (*matrix)[2][3];
-	mix->brightness /= 3;
-
-	float sin = (*matrix)[0][1] + (*matrix)[2][2];
-	float cos = (*matrix)[0][2] + (*matrix)[2][1];
-	float e = 0.001;
-	if (-e < cos && cos < e) mix->hue = M_PI;
-	else mix->hue = atanf(sin/cos);
-	mix->saturation = sqrtf(sin * sin + cos * cos) / (1.403 + 1.773);
-}
-
 VdpStatus vdp_video_mixer_set_attribute_values(VdpVideoMixer mixer,
                                                uint32_t attribute_count,
                                                VdpVideoMixerAttribute const *attributes,
@@ -223,9 +292,24 @@ VdpStatus vdp_video_mixer_set_attribute_values(VdpVideoMixer mixer,
 		return VDP_STATUS_INVALID_HANDLE;
 
 	uint32_t i;
-	for (i = 0; i < attribute_count; i++)
-		if (attributes[i] == VDP_VIDEO_MIXER_ATTRIBUTE_CSC_MATRIX)
-			set_csc_matrix(mix, (const VdpCSCMatrix *)attribute_values[i]);
+	for (i = 0; i < attribute_count; i++) {
+		switch (attributes[i]) {
+			case VDP_VIDEO_MIXER_ATTRIBUTE_CSC_MATRIX:
+				mix->custom_csc = !!attribute_values[i];
+				if (!attribute_values[i])
+				{
+					/* CSC: Use BT601 if not set */
+					color_standard = VDP_COLOR_STANDARD_ITUR_BT_601;
+					vdp_generate_csc_matrix(NULL, color_standard, &mix->csc_matrix);
+				}
+				else
+					memcpy(mix->csc_matrix, attribute_values[i], sizeof(mix->csc_matrix));
+				set_csc_matrix(mix, color_standard);
+				break;
+			default:
+				return VDP_STATUS_INVALID_VIDEO_MIXER_ATTRIBUTE;
+		}
+	}
 
 	return VDP_STATUS_OK;
 }
@@ -251,6 +335,9 @@ VdpStatus vdp_video_mixer_get_attribute_values(VdpVideoMixer mixer,
                                                VdpVideoMixerAttribute const *attributes,
                                                void *const *attribute_values)
 {
+	int i;
+	VdpCSCMatrix **vdp_csc;
+
 	if (!attributes || !attribute_values)
 		return VDP_STATUS_INVALID_POINTER;
 
@@ -258,8 +345,29 @@ VdpStatus vdp_video_mixer_get_attribute_values(VdpVideoMixer mixer,
 	if (!mix)
 		return VDP_STATUS_INVALID_HANDLE;
 
+	for (i = 0; i < attribute_count; i++) {
+		switch (attributes[i])
+		{
+			case VDP_VIDEO_MIXER_ATTRIBUTE_CSC_MATRIX:
+				vdp_csc = attribute_values[i];
+				if (!mix->custom_csc)
+					*vdp_csc = NULL;
+				else
+					memcpy(*vdp_csc, mix->csc_matrix, sizeof(VdpCSCMatrix));
+				break;
+			case VDP_VIDEO_MIXER_ATTRIBUTE_BACKGROUND_COLOR:
+			case VDP_VIDEO_MIXER_ATTRIBUTE_NOISE_REDUCTION_LEVEL:
+			case VDP_VIDEO_MIXER_ATTRIBUTE_LUMA_KEY_MIN_LUMA:
+			case VDP_VIDEO_MIXER_ATTRIBUTE_LUMA_KEY_MAX_LUMA:
+			case VDP_VIDEO_MIXER_ATTRIBUTE_SHARPNESS_LEVEL:
+			case VDP_VIDEO_MIXER_ATTRIBUTE_SKIP_CHROMA_DEINTERLACE:
+				break;
+			default:
+				return VDP_STATUS_INVALID_VIDEO_MIXER_ATTRIBUTE;
+		}
+	}
 
-	return VDP_STATUS_ERROR;
+	return VDP_STATUS_OK;
 }
 
 VdpStatus vdp_video_mixer_query_feature_support(VdpDevice device,
@@ -391,35 +499,45 @@ VdpStatus vdp_generate_csc_matrix(VdpProcamp *procamp,
                                   VdpColorStandard standard,
                                   VdpCSCMatrix *csc_matrix)
 {
-	if (!csc_matrix || !procamp)
+	if (!csc_matrix)
 		return VDP_STATUS_INVALID_POINTER;
 
-	if (procamp->struct_version > VDP_PROCAMP_VERSION)
+	if (procamp && procamp->struct_version > VDP_PROCAMP_VERSION)
 		return VDP_STATUS_INVALID_STRUCT_VERSION;
 
-	// BT.601 table
-	(*csc_matrix)[0][1] =  0.000;
-	(*csc_matrix)[0][2] =  1.403;
-
-	(*csc_matrix)[1][1] = -0.344;
-	(*csc_matrix)[1][2] = -0.714;
+	static const csc_m *cstd;
+	color_standard = standard;
+
+	switch (standard) {
+		case VDP_COLOR_STANDARD_ITUR_BT_709:
+			cstd = &cs_bt709;
+			break;
+		case VDP_COLOR_STANDARD_SMPTE_240M:
+			cstd = &cs_smpte_240m;
+			break;
+		case VDP_COLOR_STANDARD_ITUR_BT_601:
+		default:
+			cstd = &cs_bt601;
+			break;
+	}
 
-	(*csc_matrix)[2][1] =  1.773;
-	(*csc_matrix)[2][2] =  0.000;
+	float b = procamp ? procamp->brightness : 0.0f;
+	float c = procamp ? procamp->contrast : 1.0f;
+	float s = procamp ? procamp->saturation : 1.0f;
+	float h = procamp ? procamp->hue : 0.0f;
 
-	float uvcos = procamp->saturation * cosf(procamp->hue);
-	float uvsin = procamp->saturation * sinf(procamp->hue);
 	int i;
 	for (i = 0; i < 3; i++) {
-		(*csc_matrix)[i][0] = procamp->contrast;
-		float u = (*csc_matrix)[i][1] * uvcos + (*csc_matrix)[i][2] * uvsin;
-		float v = (*csc_matrix)[i][1] * uvsin + (*csc_matrix)[i][2] * uvcos;
-		(*csc_matrix)[i][1] = u;
-		(*csc_matrix)[i][2] = v;
-		(*csc_matrix)[i][3] = - (u + v) / 2;
-		(*csc_matrix)[i][3] += 0.5 - procamp->contrast / 2;
-		(*csc_matrix)[i][3] += procamp->brightness;
+		(*csc_matrix)[i][0] = c * (*cstd)[i][0];
+		(*csc_matrix)[i][1] = c * (*cstd)[i][1] * s * cosf(h) - c * (*cstd)[i][2] * s * sinf(h);
+		(*csc_matrix)[i][2] = c * (*cstd)[i][2] * s * cosf(h) + c * (*cstd)[i][1] * s * sinf(h);
+		(*csc_matrix)[i][3] = (*cstd)[i][3] + (*cstd)[i][0] * (b + c * ybias) +
+		                      (*cstd)[i][1] * (c * cbbias * s * cosf(h) + c * crbias * s * sinf(h)) +
+		                      (*cstd)[i][2] * (c * crbias * s * cosf(h) - c * cbbias * s * sinf(h));
 	}
+	VDPAU_DBG("Generate CSC matrix from following color standard: %d", standard);
+	VDPAU_DBG(">procamp->bright: %2.3f, procamp->contrast: %2.3f, procamp->saturation: %2.3f, procamp->hue: %2.3f",
+	          (double)b, (double)c, (double)s, (double)h);
 
 	return VDP_STATUS_OK;
 }
